﻿using System;
using Microsoft.Xna.Framework;

namespace MechGame.Game.PlayerRobot
{
    class RotationInterpolation
    {
        private static float WrapAngleTwoPi(float angle)
        {
            return MathHelper.WrapAngle(angle) + MathHelper.Pi;
        }

        private float _currentAngle;
        public float CurrentAngle
        {
            get { return WrapAngleTwoPi(_currentAngle); }
            set { _currentAngle = WrapAngleTwoPi(value); }
        }

        public float TargetAngle { get; private set; }

        private float _rotationSpeed;

        public RotationInterpolation(float startAngle)
        {
            CurrentAngle = startAngle;
            TargetAngle = startAngle;
            _rotationSpeed = 0;
        }

        public void SetTargetAngle(float angle, float durationToReach)
        {
            TargetAngle = WrapAngleTwoPi(angle);

            float distance;
            if (TargetAngle > CurrentAngle)
            {
                distance = TargetAngle - CurrentAngle;
                if (distance > MathHelper.Pi) distance -= MathHelper.TwoPi;
            }
            else
            {
                distance = -(CurrentAngle - TargetAngle);
                if (distance < -MathHelper.Pi) distance += MathHelper.TwoPi;
            }

            _rotationSpeed = distance/durationToReach;
        }

        public void Update(float elapsed)
        {
            if (Math.Abs(_rotationSpeed - 0.0) < 0.000000001f) return;

            CurrentAngle += elapsed*_rotationSpeed;
            if (_rotationSpeed > 0 && CurrentAngle >= TargetAngle || _rotationSpeed < 0 && CurrentAngle <= TargetAngle)
            {
                CurrentAngle = TargetAngle;
            }
        }
    }
}
